
#include "asm.h"
#include "regs.h"

.equ    KERNEL, 0
.equ    USER,   312
.equ    NESTED_COUNT, 624


LEAF(asm_start)
    mtc0    zero, CP0_STATUS
    mtc0    zero, CP0_WATCHLO
    mtc0    zero, CP0_WATCHHI

    mfc0    t0, CP0_CONFIG
    and     t0, ~0x7
    ori     t0, 0x2
    mtc0    t0, CP0_CONFIG
    
    jr  ra
END(asm_start)

.macro SAVE_CONTEXT offset
    .set    noat
    ld      $26, current_running
    addi    $26, $26, \offset
    sd      $0, OFFSET_REG0($26)
    sd      $1, OFFSET_REG1($26)
    sd      $2, OFFSET_REG2($26)
    sd      $3, OFFSET_REG3($26)
    sd      $4, OFFSET_REG4($26)
    sd      $5, OFFSET_REG5($26)
    sd      $6, OFFSET_REG6($26)
    sd      $7, OFFSET_REG7($26)
    sd      $8, OFFSET_REG8($26)
    sd      $9, OFFSET_REG9($26)
    sd      $10, OFFSET_REG10($26)
    sd      $11, OFFSET_REG11($26)
    sd      $12, OFFSET_REG12($26)
    sd      $13, OFFSET_REG13($26)
    sd      $14, OFFSET_REG14($26)
    sd      $15, OFFSET_REG15($26)
    sd      $16, OFFSET_REG16($26)
    sd      $17, OFFSET_REG17($26)
    sd      $18, OFFSET_REG18($26)
    sd      $19, OFFSET_REG19($26)
    sd      $20, OFFSET_REG20($26)
    sd      $21, OFFSET_REG21($26)
    sd      $22, OFFSET_REG22($26)
    sd      $23, OFFSET_REG23($26)
    sd      $24, OFFSET_REG24($26)
    sd      $25, OFFSET_REG25($26)
    sd      $28, OFFSET_REG28($26)
    sd      $29, OFFSET_REG29($26)
    sd      $30, OFFSET_REG30($26)
    sd      $31, OFFSET_REG31($26)

    mfc0    $27, CP0_STATUS
    sd      $27,OFFSET_STATUS($26)
    mfc0    $27, CP0_WATCHHI
    sd      $27,OFFSET_HI($26)
    mfc0    $27, CP0_WATCHLO
    sd      $27,OFFSET_LO($26)
    mfc0    $27, CP0_BADVADDR
    sd      $27,OFFSET_BADVADDR($26)
    mfc0    $27, CP0_CAUSE
    sd      $27,OFFSET_CAUSE($26)
    mfc0    $27, CP0_EPC
    sd      $27,OFFSET_EPC($26)
    
    .set    at
.endm

.macro RESTORE_CONTEXT offset
    .set    noat
    ld      $26, current_running
    addi    $26, $26, \offset
    ld      $0, OFFSET_REG0($26)
    ld      $1, OFFSET_REG1($26)
    ld      $2, OFFSET_REG2($26)
    ld      $3, OFFSET_REG3($26)
    ld      $4, OFFSET_REG4($26)
    ld      $5, OFFSET_REG5($26)
    ld      $6, OFFSET_REG6($26)
    ld      $7, OFFSET_REG7($26)
    ld      $8, OFFSET_REG8($26)
    ld      $9, OFFSET_REG9($26)
    ld      $10, OFFSET_REG10($26)
    ld      $11, OFFSET_REG11($26)
    ld      $12, OFFSET_REG12($26)
    ld      $13, OFFSET_REG13($26)
    ld      $14, OFFSET_REG14($26)
    ld      $15, OFFSET_REG15($26)
    ld      $16, OFFSET_REG16($26)
    ld      $17, OFFSET_REG17($26)
    ld      $18, OFFSET_REG18($26)
    ld      $19, OFFSET_REG19($26)
    ld      $20, OFFSET_REG20($26)
    ld      $21, OFFSET_REG21($26)
    ld      $22, OFFSET_REG22($26)
    ld      $23, OFFSET_REG23($26)
    ld      $24, OFFSET_REG24($26)
    ld      $25, OFFSET_REG25($26)
    ld      $28, OFFSET_REG28($26)
    ld      $29, OFFSET_REG29($26)
    ld      $30, OFFSET_REG30($26)
    ld      $31, OFFSET_REG31($26)
    
    ld      $27,OFFSET_STATUS($26)
    mtc0    $27, CP0_STATUS
    ld      $27,OFFSET_HI($26)
    mtc0    $27, CP0_WATCHHI
    ld      $27,OFFSET_LO($26)
    mtc0    $27, CP0_WATCHLO
    ld      $27,OFFSET_BADVADDR($26)
    mtc0    $27, CP0_BADVADDR
    ld      $27,OFFSET_CAUSE($26)
    mtc0    $27, CP0_CAUSE
    ld      $27,OFFSET_EPC($26)
    mtc0    $27, CP0_EPC

   .set    at
.endm

# function do_scheduler
NESTED(do_scheduler, 0, ra)
    SAVE_CONTEXT(KERNEL)
    jal     scheduler
    RESTORE_CONTEXT(KERNEL)
    jr      ra
END(do_scheduler)

NESTED(scheduler_first, 0, ra)
    RESTORE_CONTEXT(USER)
    eret
END(scheduler_first)

NESTED(scheduler_again, 0, ra)
    RESTORE_CONTEXT(KERNEL)
    jr      ra
END(scheduler_again)



LEAF(reset_timer)
    mtc0    zero, CP0_COUNT
    mtc0    a0, CP0_COMPARE
    jr      ra
END(reset_timer)

LEAF(set_cp0_status)
    mtc0    a0, CP0_STATUS
    jr      ra
END(set_cp0_status)

LEAF(set_cp0_cause)
    mtc0    a0, CP0_CAUSE
    jr      ra
END(set_cp0_cause)

LEAF(set_cp0_compare)
    mtc0    a0, CP0_COMPARE
    jr      ra
END(set_cp0_compare)

LEAF(set_cp0_count)
    mtc0    a0, CP0_COUNT
    jr      ra
END(set_cp0_count)
    
LEAF(get_cp0_status)
    mfc0    v0, CP0_STATUS
    jr      ra
END(get_cp0_status)
    
LEAF(get_cp0_cause)
    mfc0    v0, CP0_CAUSE
    jr      ra
END(get_cp0_cause)

LEAF(get_cp0_count)
	mfc0    v0, CP0_COUNT
    jr      ra
END(get_cp0_count)

LEAF(get_cp0_compare)
	mfc0    v0, CP0_COMPARE
    jr      ra
END(get_cp0_compare)


 
.global exception_handler_begin
.global exception_handler_end
.global TLBexception_handler_begin
.global TLBexception_handler_end
.global return_to_user

#tlb refill
NESTED(TLBexception_handler_entry, 0, sp)
TLBexception_handler_begin:
    dla     k0, handle_TLB_Refill
    jr      k0
TLBexception_handler_end:
END(TLBexception_handler_entry)

NESTED(exception_handler_entry, 0, sp)   
exception_handler_begin:
    mfc0    k0, CP0_STATUS
    mfc0    k0, CP0_COUNT
    mfc0    k0, CP0_COMPARE
    SAVE_CONTEXT(USER)
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    ld      sp, OFFSET_REG29(k0)
    
    mfc0    k1, CP0_CAUSE
    andi    k1, k1, CAUSE_EXCCODE
    dla		k0, exception_handler
    add     k0, k0, k1
    add     k0, k0, k1
    ld      k0, 0(k0)
    jr      k0
exception_handler_end:
END(exception_handler_entry)

NESTED(handle_int, 0, sp)
    mfc0	a0, CP0_STATUS
	mfc0	a1, CP0_CAUSE
    jal     interrupt_helper
return_to_user:
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    sd      sp, OFFSET_REG29(k0)
    RESTORE_CONTEXT(USER)
    #jal     change_state
    eret
END(handle_int)

NESTED(handle_syscall, 0, sp)
    add     a3, a2, zero
    add     a2, a1, zero
    add     a1, a0, zero
    add     a0, v0, zero
    jal     system_call_helper
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    sd      sp, OFFSET_REG29(k0)
    RESTORE_CONTEXT(USER)
    eret  
END(handle_syscall)

#TLB Invalid
NESTED(handle_tlb, 0, sp)
    jal     do_TLB_Invalid
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    sd      sp, OFFSET_REG29(k0)

    RESTORE_CONTEXT(USER)
    eret
END(handle_tlb)

#TLB Refill
NESTED(handle_TLB_Refill, 0, sp)
    SAVE_CONTEXT(USER)
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    ld      sp, OFFSET_REG29(k0)
    
    jal     do_TLB_Refill
    #CHANGE STACK
    ld      k0, current_running
    addi    k0, k0, KERNEL
    sd      sp, OFFSET_REG29(k0)

    RESTORE_CONTEXT(USER)
    eret
END(handle_TLB_Refill)

NESTED(handle_other, 0, sp)
    jal     handle_other_exception
    RESTORE_CONTEXT(USER)
    eret
END(handle_other)

LEAF(exception_handler_exit)
  
END(exception_handler_exit)


LEAF(get_cp0_entryhi)
    dmfc0    v0, CP0_ENTRYHI
    jr      ra
END(get_cp0_entryhi)


LEAF(set_cp0_entryhi)
    dmtc0    a0, CP0_ENTRYHI
    jr      ra
END(set_cp0_entryhi)


LEAF(get_cp0_index)
    mfc0    v0, CP0_INDEX 
    jr      ra
END(get_cp0_index)

LEAF(set_cp0_index)
    mtc0    a0, CP0_INDEX
    jr      ra
END(set_cp0_index)

LEAF(get_cp0_badvaddr)
    
END(get_cp0_badvaddr)

LEAF(get_cp0_entrylo0)
    dmfc0    v0, CP0_ENTRYLO0 
    jr      ra
END(get_cp0_entrylo0)
    
LEAF(set_cp0_entrylo0)
    dmtc0    a0, CP0_ENTRYLO0
    jr      ra
END(set_cp0_entrylo0)

LEAF(get_cp0_entrylo1)
    dmfc0    v0, CP0_ENTRYLO1
    jr      ra
END(get_cp0_entrylo1)

LEAF(set_cp0_entrylo1)
    dmtc0    a0, CP0_ENTRYLO1
    jr      ra
END(set_cp0_entrylo1)

LEAF(set_cp0_pagemask)
    dmtc0    a0, CP0_PAGEMASK
    jr      ra
END(set_cp0_pagemask)

LEAF(get_cp0_context)
    dmfc0    v0, CP0_CONTEXT
    jr      ra
END(get_cp0_context)

LEAF(tlbwr_operation)
   
END(tlbwr_operation)

LEAF(tlbwi_operation)
    
END(tlbwi_operation)

LEAF(tlbp_operation)
    
END(tlbp_operation)



LEAF(get_cp0_config)
    
END(get_cp0_config)

LEAF(get_cpu_id)
    
END(get_cpu_id)

